/*
  Program Name:  MC_Main
  Project:       LLH Hopscotch
  Written By:    Daniel Lee
  Revision:      0.7.2a
  Change log:   
    0.6.1...    Adding console debugging
        Adding code to send manual commands from the serial console
        for test the various functions of the network.
    0.6.3...    Interruptible Auto-mode is now functional
    0.7.0...    Implementing "Manual" and "Program" modes
    0.7.1...    In preparation for 0.8.0 this revision is designed to
        allow for simplified command line inputs. These inputs 
        will be precursors to the gamepad inputs intended for implementation
        in 0.8.0. This revision now accepts the ID of the current AUTO mode
        tile and stores it in auto_ID.
        
        Bug note: leaving the network on for while with no input can sometimes
        cause the MC_Main to freeze. cause is currently unknown
    0.7.2a...   This revision number is to reflect that it is a descendant of
        the MC_Main0.7.1.ino file but is distinct from the other MC_Main0.7.2 
        family. This revision will attempt to address the MC_Main freeze bug. 
        This revision has enabled simple Up/Down/Left/Right cursor movement 
        from the serial console.
*/

#include<SoftwareSerial.h>


// Pins 2 and 7 are reserved for FTDI comms
// Pins 0 and 1 are reserved for USB comms (during programming only)

#define TRXRATE 19200
#define TERMBAUD 9600
#define LEDPIN 3
#define SENSORPIN A5
#define SPKRPIN 11
#define SEEDPIN A1
#define RX_boards 19
#define TX_boards 18
#define MAX_BUFFER_SIZE 64
#define MAX_NET_LEN 16
#define NET_ENTRY_LEN 9
#define DIR_OFFSET 5
#define TIME_OUT 25
#define MSG_RCVD 102
#define ID_REQUEST 103
#define SETUP_ID 104
#define SENDING 0
#define READING 1
#define COMPLETE 2
#define CONFIRM_IDSET 3

#define LISTENING 0
#define CHANNEL_SET 1
#define PROCESS_MSG 2
//COMMAND CODES:
#define COM 170
#define ROGER 171
#define NEGATIVE 172
#define SETID 173
#define SENDID 174
#define SETNEIGHBORS 175
#define IDSET 176
#define NEIGHBORSSET 177
#define STATE_CHANGE 178
#define CURSOR 179
#define AMCURSOR 180
#define SEND_CURSOR 181
#define STEP_ON_ME 182
#define MANUAL 183
#define PROGRAM 184
#define AMAUTO 185
#define IDSENT 186
#define SEND_CURSOR_NORTH 187
#define SEND_CURSOR_EAST 188
#define SEND_CURSOR_SOUTH 189
#define SEND_CURSOR_WEST 190
#define AUTO 200
#define NOCOM 255

//SoftwareSerial board_port(RX_1,TX_1);

int active_channel;
int listen_count;
int state;
long prevTime;
int ledState;
uint8_t myID;
uint8_t cursor_ID;
uint8_t auto_ID;
uint8_t north_ID;
uint8_t south_ID;
uint8_t east_ID;
uint8_t west_ID;
uint8_t board_ID;
uint8_t read_buf[MAX_BUFFER_SIZE];
uint8_t write_buf[MAX_BUFFER_SIZE];
int read_buf_len;
int write_buf_len;
int send_status;
int next_available_ID;
int net_map[MAX_NET_LEN][NET_ENTRY_LEN];
int net_map_len;
uint8_t con_buf[MAX_BUFFER_SIZE];
int con_buf_len;
int target_ID; //Was causing cross init errors in switch statement, had to move it.
int dir_val,dir_index,dir_done,dir_cnt,prev_dir_cnt;


void blink_led(int ledPin){
  long blinkTime = 250;
  long curTime = millis();
  if((curTime - prevTime) > blinkTime){
    prevTime = curTime;
    if(ledState == LOW){
      ledState = HIGH;
    }else{
      ledState = LOW;
    }
  }
  digitalWrite(ledPin, ledState);
}

void writebuffertoboards(uint8_t *buf, int buf_len){
    const uint8_t start_sig = COM;
    int temp_len = buf_len;
    uint8_t temp_buf[buf_len+1];
    while(temp_len > 0){
        temp_buf[temp_len] = buf[temp_len-1];
        temp_len--;
    }
    temp_buf[0] = start_sig;
    Serial.print("Writing");
    for(int i=0;i<buf_len+1;i++){
      Serial.print(", ");
      Serial.print(temp_buf[i], DEC);
    }
    Serial.println(" to serial the board network.");
    Serial1.write(temp_buf, buf_len+1);
}

void read_from_boards(){
  if(Serial1.read() == COM){
    int cnt=0;
    uint8_t cur_read = -1;
    while(Serial1.available() > 0 && cnt < MAX_BUFFER_SIZE && cur_read != COM){
      cur_read = Serial1.read();
      read_buf[cnt] = cur_read;
      cnt++;
    }
    read_buf_len = cnt;
    send_msg_received();
  }else{
    Serial1.flush();
  }
}

int send_msg(){
  int count = 0;
  int confirm_rcvd = 0;
  int temp_buf_len = 0;
  uint8_t temp_buf[MAX_BUFFER_SIZE];
  while(confirm_rcvd != 1 && count < TIME_OUT){
    writebuffertoboards(write_buf, write_buf_len);
    delay(30);
    if(Serial1.read() == COM){
        int cnt = 0;
        while(Serial1.available() > 0 && Serial1.peek() != COM){
            temp_buf[cnt] = Serial1.read();
            cnt++;
        }
        temp_buf_len = cnt;
        if(cnt > 0 && temp_buf[cnt-1] == ROGER){
          confirm_rcvd = 1;
          Serial.println("...");
        }else{
          while(Serial1.available() > 0)Serial1.read();
        }
    }else{
        //if the input buffer doesn't start with '170', flush the garbage.
        read_buf_len = 0;
        while(Serial1.available() > 0)Serial1.read();
    }
    count++;
  }
  delay(60);
  while(Serial1.available() > 0)Serial1.read();
  read_buf_len = 0;
  
  if(count >= TIME_OUT || temp_buf_len <= 0){
    return -1;
  }else{
    return 0;
  }
}

void send_msg_received(){
    uint8_t msg_rcvd = ROGER;
    for(int i=0;i<2;i++){
        writebuffertoboards(&msg_rcvd, 1);
        delayMicroseconds(500);
    }
    //returning with the assumption the our confirmation was received
}

int find_next_setup_ID(){
    //Set up IDs (id selection):
    int temp_row = net_map_len - 1;
    if (temp_row < 0){
        return 1;
    }else{
        //check the current row for an ID larger than the row's initial ID
        int test_ID = 0;
        while(test_ID == 0 && temp_row >= 0){
            int row_ID = net_map[temp_row][0];
            int i=0;
            int ID_found = 0;
            while(ID_found == 0 && i < 4){
                if(net_map[temp_row][i+1] < 255 && net_map[temp_row][i+1] > row_ID){
                    int temp_ID = net_map[temp_row][i+1];
                    //now check to make sure temp_ID doesn't already have an entry in the net_map
                    int entry_exists = 0;
                    for(int j=0;j<net_map_len;j++){
                        if(net_map[j][0] == temp_ID){
                            //ID already has a map entry
                            entry_exists = 1;
                        }
                    }
                    if(entry_exists == 0){
                        ID_found = 1;
                        test_ID = temp_ID;
                    }
                }
                i++;
            }
            temp_row--;
        }
        return test_ID;
    }
}

void prep_remote_msg(int target_ID){
    int path_ID = target_ID;
    write_buf_len = 0;
    if(target_ID > 1){
        Serial.print("Adding ");
        Serial.print(target_ID);
        Serial.println(" to write_buf");
        write_buf[0] = target_ID;
        write_buf_len = 1;
        //look for target ID among the ports (columns 1 thru 4) of the net_map array
        int j=0;
        int k=0;
        int done = 0;
        int found_ID = 0;
        int dir = 1;
        while(done == 0){
            for(k=0;k<4;k++){
                Serial.println(net_map[j][k+1]);
                if(net_map[j][k+1] == path_ID){
                    dir = -1;
                    found_ID = 1;
                }
            }
            j += dir;
            if(found_ID == 1){
                int index = j+1;
                //ID was found at net_map[index][0]
                path_ID = net_map[index][0];
                Serial.print("found ID: ");
                Serial.print(path_ID);
                Serial.print(" at index: ");
                Serial.println(index);
                if(path_ID > 1){
                    //shift it in to the write buffer
                    int cnt = write_buf_len;  //at this point write_buf_len is at least  '1'
                    while(cnt > 0){
                        write_buf[cnt] = write_buf[cnt-1];
                        cnt--;
                    }
                    Serial.print("Adding ");
                    Serial.print(path_ID);
                    Serial.println(" to write_buf");
                    write_buf[cnt] = path_ID;
                    write_buf_len++;
                }else{
                    done = 1;
                }
                found_ID = 0;
            }
        }
    }
    Serial.print("write_buf_len: ");
    Serial.println(write_buf_len);
    Serial.println("write_buf:");
    for(int q=0;q<write_buf_len;q++){
      Serial.print(write_buf[q]);
    }
    Serial.println("");
}

//only for use with the setneighbors command
void append_return_path(int target_ID){
    //write_buf[write_buf_len-3] should be the target ID
    int cnt = write_buf_len-4;
    while(cnt >= 0){
        write_buf[write_buf_len] = write_buf[cnt];
        cnt--;
        write_buf_len++;
    }
    if(target_ID > 1){
        write_buf[write_buf_len] = 1;
        write_buf_len++;
    }
    write_buf[write_buf_len] = 0;
    write_buf_len++;
}
void send_cursor_dir(int dir){
  int dir_ID = 0;
  //first determine cursor_ID's northern neighbor
  for(int ii=0;ii<net_map_len;ii++){        //this is gross but it gets the job done
    if(net_map[ii][0] == cursor_ID){
      for(int jj=0;jj<4;jj++){
        if(net_map[ii][jj+DIR_OFFSET] == dir){
          //jj+DIR_OFFSET-4 points to the port with north_ID
          dir_ID = net_map[ii][jj+DIR_OFFSET-4];
        }
      }
    }
  }
  if(dir_ID != 255 && dir_ID > 0){
    //tell the current cursor_ID to change state to Listening
    prep_remote_msg(cursor_ID);
    write_buf[write_buf_len] = STATE_CHANGE;
    write_buf_len++;
    write_buf[write_buf_len] = LISTENING;
    write_buf_len++;
    send_msg();
     
    //send to dir_ID
    prep_remote_msg(dir_ID);
    write_buf[write_buf_len] = CURSOR;
    write_buf_len++;
    send_status = send_msg();
    if(send_status != 0){
      Serial.print("Error: status code = ");
      Serial.print(send_status);
      Serial.println(". Error with SEND_CURSOR_NORTH!!");
    }else{
      cursor_ID = dir_ID;
    }
  }
}

void setup(){
  Serial.begin(TERMBAUD);
  Serial.println("attempting to initialize Serial comms...");
  Serial.println("...");
  Serial1.begin(TRXRATE);
  pinMode(LEDPIN, OUTPUT);
  Serial.println("...initialization complete.");
  randomSeed(analogRead(SEEDPIN));
  myID = 0;
  cursor_ID = 0;
  auto_ID = 0;
  board_ID = -1;
  active_channel = 1;
  listen_count = 1;
  digitalWrite(LEDPIN, HIGH);
  state = SENDING;
  read_buf_len = 0;
  write_buf_len = 0;
  for(int j=0;j<MAX_NET_LEN;j++){
    for(int k=0;k<NET_ENTRY_LEN;k++){
      net_map[j][k] = -1;
    }
  }
  net_map_len = 0;
  ledState = LOW;
  next_available_ID = 1;
  prevTime = millis();
  //send_status = -1;
}

void loop(){
  switch(state){
    case SENDING:
      write_buf[0] = SENDID;
      write_buf_len = 1;
      send_status = send_msg();
      if(send_status != 0){
      //something went wrong with board to board comms
        Serial.print("Error: status code = ");
        Serial.print(send_status);
        Serial.println(". Something went wrong with board-to-board comms!!!");
      }else{
        state = READING;
      }
      break;
      
    case READING:
      delay(1);
      read_buf_len = 0;  //this might just break the whole universe
      if(Serial1.read() == COM){
        Serial.println("COM");
        //uint8_t cur_read = -1;
        int cnt = 0;
        while(Serial1.available() > 0 && cnt < MAX_BUFFER_SIZE && (uint8_t)Serial1.peek() != COM){
          //cur_read = Serial1.read();
          read_buf[cnt] = Serial1.read(); //cur_read;
          cnt++;
          Serial.println("available");
          delay(1);
        }
        read_buf_len = cnt;
      }
      if(read_buf_len > 0){
        send_msg_received();
        Serial.println("Received message from boards:");
        for(int i=0;i<read_buf_len;i++){
          Serial.print(read_buf[i]);
        }
        Serial.println("");
        switch((int)read_buf[0]){
          case IDSET:
            board_ID = read_buf[1];
            next_available_ID = board_ID + 1;
            state = SETNEIGHBORS;
            break;
          case NEIGHBORSSET:
            if(read_buf_len == 7){
              dir_val=dir_index=dir_done=dir_cnt=prev_dir_cnt=0;
              for(int i=0;i<5;i++){
                net_map[net_map_len][i] = read_buf[i+1];
                //determine orientation data
                if(net_map_len > 0){
                  Serial.print("net_map_len=");
                  Serial.println(net_map_len);
                  Serial.println("net_map_len > 0");
                  if(i>0){
                    Serial.print("i=");
                    Serial.println(i);
                    Serial.println("i > 0");
                    dir_cnt=0;
                    Serial.print("dir_done=");
                    Serial.println(dir_done);
                    Serial.print("dir_cnt=");
                    Serial.println(dir_cnt);
                    while(dir_done != 1 && dir_cnt < net_map_len){
                      Serial.println("dir_done != 1 && dir_cnt < net_map_len");
                      if(net_map[dir_cnt][0] == net_map[net_map_len][i]){
                        Serial.print("net_map[");
                        Serial.print(dir_cnt);
                        Serial.print("][0] == net_map[");
                        Serial.print(net_map_len);
                        Serial.print("][");
                        Serial.print(i);
                        Serial.println("]");
                        prev_dir_cnt = 1;
                        while(dir_done != 1 && prev_dir_cnt < 5){
                          if(net_map[dir_cnt][prev_dir_cnt] == net_map[net_map_len][0]){
                            Serial.print("net_map[");
                            Serial.print(dir_cnt);
                            Serial.print("][");
                            Serial.print(prev_dir_cnt);
                            Serial.print("] == net_map[");
                            Serial.print(net_map_len);
                            Serial.println("][0]");
                            dir_val = (net_map[dir_cnt][prev_dir_cnt+4] + 2) % 4;
                            dir_index = i + 4 - DIR_OFFSET;
                            for(int j=0;j<4;j++){
                              net_map[net_map_len][((dir_index + j) % 4) + DIR_OFFSET] = (dir_val + j) % 4;
                            }
                            dir_done = 1;
                          }
                          prev_dir_cnt++;
                        }
                      }
                      dir_cnt++;
                    }
                  }
                }else{
                  //this must be the first entry in net_map
                  if(net_map[net_map_len][i] == 0){
                    dir_val = 2;
                    dir_index = i + 4 - DIR_OFFSET;
                    for(int j=0;j<4;j++){
                      net_map[net_map_len][((dir_index + j) % 4) + DIR_OFFSET] = (dir_val + j) % 4;
                    }
                  }
                }
              }
              net_map_len++;
              next_available_ID = read_buf[6];
              state = SETNEIGHBORS;
            }else{
              //we didn't get the whole message.
              Serial.print("Read Buffer Length = ");
              Serial.println(read_buf_len);
            }
            break;
          case SENDID:
            send_status = -1;
            write_buf[0] = IDSENT;
            write_buf[1] = myID;
            write_buf_len = 2;
            send_status = send_msg();
            if(send_status != 0){
              //something went wrong with board to board comms
              Serial.print("Error: status code = ");
              Serial.print(send_status);
              Serial.println(". Something went wrong with board-to-board comms!!!");
            }else{
              state = READING;
            }
            break;
          case AMCURSOR:
            cursor_ID = read_buf[1];
            Serial.println("AMCURSOR");
            Serial.print("cursor_ID: ");
            Serial.println(cursor_ID);
            state = READING;
            break;
          case AMAUTO:
            auto_ID = read_buf[1];
            Serial.println("AMAUTO");
            Serial.print("auto_ID: ");
            Serial.println(auto_ID);
            state = READING;
            break;
          case IDSENT:
            board_ID = read_buf[1];
            state = SETID;
            break;
          default:
            state = READING;
            break;
        }
      }
      //Check for input from the console
      con_buf_len = 0;
      while(Serial.available() > 0){
        con_buf[con_buf_len] = Serial.parseInt();
        con_buf_len++;
      }
      //if there was a message from the console it will be
      //stored in con_buf
      if(con_buf_len > 0){
        switch((int)con_buf[0]){
          case STATE_CHANGE:
            state = con_buf[1];
            break;
          case SEND_CURSOR_NORTH:
            send_cursor_dir(0);
            break;
          case SEND_CURSOR_EAST:
            send_cursor_dir(1);
            break;
          case SEND_CURSOR_SOUTH:
            send_cursor_dir(2);
            break;
          case SEND_CURSOR_WEST:
            send_cursor_dir(3);
            break;
          default:
            for(int i=0;i<con_buf_len;i++){
              write_buf[i] = con_buf[i];
            }
            write_buf_len = con_buf_len;
            send_msg();
            state = READING;
            break;
        }
      }
    //while(Serial1.available() > 0)Serial1.read();
    //Serial.println("%%%");
    break;
    
  case COMPLETE:
    //Serial.println("Auto signal sent!");
    Serial.print("My ID is: ");
    Serial.println(myID);
    Serial.print("My Neighbor ID is: ");
    Serial.println(board_ID);
    Serial.println("System map:");
    for(int j=0;j<net_map_len;j++){
      for(int k=0;k<NET_ENTRY_LEN;k++){
        Serial.print(net_map[j][k]);
        Serial.print(", ");
      }
      Serial.println("");
    }
    Serial.println("***** SETUP COMPLETE *****");
    state = READING;
    /*
    //Check for input from the console
    con_buf_len = 0;
    while(Serial.available() > 0){
      con_buf[con_buf_len] = Serial.parseInt();
      con_buf_len++;
    }
    //if there was a message from the console it will be
    //stored in con_buf
    if(con_buf_len > 0){
      if(con_buf[0] == STATE_CHANGE){
        state = con_buf[1];
      }else{
        for(int i=0;i<con_buf_len;i++){
          write_buf[i] = con_buf[i];
        }
        write_buf_len = con_buf_len;
        send_msg();
        state = READING;
      }
    }
    delay(9);
    */
    break;
    
  case SETID:
    Serial.println("ID request successful!");
    Serial.print("My ID is: ");
    Serial.println(myID);
    Serial.print("My Neighbor ID is: ");
    Serial.println(board_ID);
    Serial.println("***** TEST COMPLETE *****");
    write_buf[0] = SETID;
    write_buf[1] = next_available_ID;
    write_buf_len = 2;
    send_status = send_msg();
    if(send_status != 0){
      Serial.print("Error: status code = ");
      Serial.print(send_status);
      Serial.println(". Something went wrong with board-to-board comms!!!");
    }else{
      state = READING;
    }
    break;
    
 case SETNEIGHBORS:
    target_ID = 0;
    Serial.println("Set ID successful!");
    Serial.print("My ID is: ");
    Serial.println(myID);
    Serial.print("My Neighbor ID is now: ");
    Serial.println(board_ID);
    Serial.println("***** TEST COMPLETE *****");
    target_ID = find_next_setup_ID();
    if(target_ID == 0){
        while(Serial1.available() > 0)Serial1.read();
        read_buf_len = 0;
        state = COMPLETE;
    }else{
        prep_remote_msg(target_ID);
        write_buf[write_buf_len] = SETNEIGHBORS;
        write_buf[write_buf_len+1] = next_available_ID;
        write_buf_len = write_buf_len+2;
        append_return_path(target_ID);
        send_status = send_msg();
        if(send_status != 0){
            Serial.print("Error: status code = ");
            Serial.print(send_status);
            Serial.println(". Something went wrong with board-to-board comms!!!");
        }else{
            state = READING;
        }
    }
    break;
    
 case AUTO:
 
    Serial.println("Set Neighbors Successful!");
    Serial.print("My ID is: ");
    Serial.println(myID);
    Serial.print("My Neighbor ID is: ");
    Serial.println(board_ID);
    Serial.println("System map:");
    for(int j=0;j<net_map_len;j++){
      for(int k=0;k<5;k++){
        Serial.print(net_map[j][k]);
        Serial.print(", ");
      }
      Serial.println("");
    }
    Serial.println("***** TEST COMPLETE *****");
    Serial.println("Beginning autonomous test!");
    //select the starting board from all available boards in the net_map
    //start by creating an array of all known IDs
    int start_ID;
    int list_of_IDs[MAX_NET_LEN];
    int list_len;
    int rand_index;
    for(list_len=0;list_len<net_map_len;list_len++){
        list_of_IDs[list_len] = net_map[list_len][0];
    }
    //choose an index at random
    rand_index = (int)random(list_len);
    start_ID = list_of_IDs[rand_index];
    //send the AUTO command to the chosen ID
    prep_remote_msg(start_ID);
    write_buf[write_buf_len] = AUTO;
    write_buf_len++;
    Serial.print("Sending autonomous mode signal to ID = ");
    Serial.println(start_ID);
    send_msg();
    //Once the message is sent, no response from the network is currently being expected
    //this may change in future versions
    state = READING;
    break;
  case MANUAL:
    //first, tell the current auto_ID to change state to Listening
    prep_remote_msg(auto_ID);
    write_buf[write_buf_len] = STATE_CHANGE;
    write_buf_len++;
    write_buf[write_buf_len] = LISTENING;
    write_buf_len++;
    send_msg();
    write_buf[0] = CURSOR;
    write_buf_len = 1;
    send_status = send_msg();
    if(send_status != 0){
      Serial.print("Error: status code = ");
      Serial.print(send_status);
      Serial.println(". Something went wrong with board-to-board comms!!!");
    }else{
      state = READING;
    }
    break;
  case PROGRAM:
    break;
  default:
    break;
  }
}
